#include "arduino.h"
#define QUE_DEPTH 200
class queue_16 {
  public:
  unsigned long data[QUE_DEPTH];
  int wr_pt;
  int rd_pt;
  
  boolean wr_flag;
  boolean rd_flag;

  unsigned long cnt;

  void reset() {
    wr_pt = 0;
    rd_pt = 0;
    wr_flag = 0;
    rd_flag = 0;
    cnt = 0;
  }

  int get_depth() {
    if(wr_flag == rd_flag) {
      return wr_pt - rd_pt;
    }
    else {
      return QUE_DEPTH + wr_pt - rd_pt;
    }
  }

  boolean push(unsigned long wdat) {
    int depth=-1;
    depth = get_depth();
    if(depth >= QUE_DEPTH) {
      Serial.println("ERROR:Queue is full,can't be push!");
      return false;
    }
    else {
      if(wr_pt >= QUE_DEPTH) {
        wr_pt = 0;
        wr_flag = !wr_flag;
      }
      data[wr_pt] = wdat;
      wr_pt++;
      return true;
    }
  }
  
  boolean pop(unsigned long &rdat) {
    int depth=-1;
    depth = get_depth();
    if(depth == 0) {
      Serial.println("ERROR:Queue is empty,can't be pop!");
      return false;
    }
    else {
      if(rd_pt >= QUE_DEPTH) {
        rd_pt = 0;
        rd_flag = !rd_flag;
      }
      rdat = data[rd_pt];
      rd_pt = rd_pt + 1;
      cnt = cnt + 1;
      return true;
    }
  }

  void retreat() {
    int depth=-1;
    depth = get_depth();
    if(depth > 0) {
      if(rd_pt == 0) {
        rd_pt = QUE_DEPTH - 1;
	rd_flag = !rd_flag;
      }
      else {
        rd_pt = rd_pt - 1;
      }
      if(cnt > 0) {
        cnt = cnt - 1;
      }
    }
  }
};
